#include "stdafx.h"
#include <cmath>
#include "Intersection.h"
#include "planevolume.h"
#include "linevolume.h"

typedef Plane<float> Planef;
typedef PlaneVolume<float> PlaneVolumef;

#pragma warning (push)
#pragma warning (disable: 4100)

namespace Intersection
{
    // lab 1
    bool PointInsideSphere(const Sphere& aSphere,const Vector3f& aTestPoint)
    {
    	float sqDist = (aSphere.myCenterPosition.x-aTestPoint.x)*(aSphere.myCenterPosition.x-aTestPoint.x)+(aSphere.myCenterPosition.y-aTestPoint.y)*(aSphere.myCenterPosition.y-aTestPoint.y)+(aSphere.myCenterPosition.z-aTestPoint.z)*(aSphere.myCenterPosition.z-aTestPoint.z);
    	return (sqDist<=aSphere.myRadiusSquared);
    }

    bool PointInsideAABB(const AABB& anAABB,const Vector3f& aTestPoint)
    {
    	if((aTestPoint.x>=anAABB.myMinPos.x) && (aTestPoint.x<=anAABB.myMaxPos.x) &&
			(aTestPoint.y>=anAABB.myMinPos.y) && (aTestPoint.y<=anAABB.myMaxPos.y) &&
			(aTestPoint.z>=anAABB.myMinPos.z) && (aTestPoint.z<=anAABB.myMaxPos.z))
			return true;
    	return false;
    	//return (aTestPoint == ClosestPointAABB(aTestPoint, anAABB));
    }

	Vector3f ClosestPointAABB(const Vector3f &aPoint, const AABB &anAABB)
	{
		Vector3f point(aPoint);
		if(aPoint.x < anAABB.myMinPos.x)
		{
			point.x = anAABB.myMinPos.x;
		}
		else if(aPoint.x > anAABB.myMaxPos.x)
		{
			point.x = anAABB.myMaxPos.x;
		}

		if(aPoint.y < anAABB.myMinPos.y)
		{
			point.y = anAABB.myMinPos.y;
		}
		else if(aPoint.y > anAABB.myMaxPos.y)
		{
			point.y = anAABB.myMaxPos.y;
		}

		if(aPoint.z < anAABB.myMinPos.z)
		{
			point.z = anAABB.myMinPos.z;
		}
		else if(aPoint.z > anAABB.myMaxPos.z)
		{
			point.z = anAABB.myMaxPos.z;
		}
		return point;
	}

    bool PointInsideOBB(const OBB& anOBB,const Vector3f& aTestPoint)
    {
    	PlaneVolumef pv;
    	pv.AddPlane(Planef(anOBB.myPoints[0], anOBB.myPoints[1], anOBB.myPoints[1]));
    	pv.AddPlane(Planef(anOBB.myPoints[1], anOBB.myPoints[5], anOBB.myPoints[6]));
    	pv.AddPlane(Planef(anOBB.myPoints[4], anOBB.myPoints[0], anOBB.myPoints[3]));
    	pv.AddPlane(Planef(anOBB.myPoints[4], anOBB.myPoints[5], anOBB.myPoints[1]));
    	pv.AddPlane(Planef(anOBB.myPoints[3], anOBB.myPoints[2], anOBB.myPoints[6]));
    	pv.AddPlane(Planef(anOBB.myPoints[7], anOBB.myPoints[6], anOBB.myPoints[5]));
    	return pv.Inside(aTestPoint);
    }


    bool LineVsLine(const LineSegment2D& aFirstLine, const LineSegment2D& aSecondLine, Vector3f& anIntersectionPoint)
    {
		__asm int 3;
		assert(false && "This line vs line is bugged. Use LineVsLineRobStyle instead");
    	float denom = (aSecondLine.myEndPos.y-aSecondLine.myStartPos.y)*(aFirstLine.myEndPos.x-aFirstLine.myStartPos.x)-(aSecondLine.myEndPos.x-aSecondLine.myStartPos.x)*(aFirstLine.myEndPos.y-aFirstLine.myStartPos.y);
    	if(denom==0)
    	{
    		return false;
    	}
    	denom = 1/denom;
    	float ua = ((aSecondLine.myEndPos.x-aSecondLine.myStartPos.x)*(aFirstLine.myStartPos.y-aSecondLine.myStartPos.y)-(aSecondLine.myEndPos.y-aSecondLine.myStartPos.y)*(aFirstLine.myStartPos.x-aSecondLine.myStartPos.x))*denom;
    	if((ua<0) || (ua>1))
    	{
    		return false;
    	}
    	anIntersectionPoint.Set(aFirstLine.myStartPos.x+ua*(aFirstLine.myEndPos.x-aFirstLine.myStartPos.x), aFirstLine.myStartPos.y+ua*(aFirstLine.myEndPos.y-aFirstLine.myStartPos.y), 0);
    	return true;
    }


	bool LineVsLineRobStyle(const LineSegment2D& aFirstLine,const LineSegment2D& aSecondLine,Vector3f& anIntersectionPoint)
	{
		const float ua = ((aSecondLine.myEndPos.x - aSecondLine.myStartPos.x) * (aFirstLine.myStartPos.y - aSecondLine.myStartPos.y) - 
                (aSecondLine.myEndPos.y - aSecondLine.myStartPos.y) * (aFirstLine.myStartPos.x - aSecondLine.myStartPos.x)) /
                ((aSecondLine.myEndPos.y - aSecondLine.myStartPos.y) * (aFirstLine.myEndPos.x - aFirstLine.myStartPos.x) - 
                (aSecondLine.myEndPos.x - aSecondLine.myStartPos.x) * (aFirstLine.myEndPos.y - aFirstLine.myStartPos.y));
 
        const float ub = ((aFirstLine.myEndPos.x - aFirstLine.myStartPos.x) * (aFirstLine.myStartPos.y - aSecondLine.myStartPos.y) - 
                (aFirstLine.myEndPos.y - aFirstLine.myStartPos.y) * (aFirstLine.myStartPos.x - aSecondLine.myStartPos.x)) /
                ((aSecondLine.myEndPos.y - aSecondLine.myStartPos.y) * (aFirstLine.myEndPos.x - aFirstLine.myStartPos.x) - 
                (aSecondLine.myEndPos.x - aSecondLine.myStartPos.x) * (aFirstLine.myEndPos.y - aFirstLine.myStartPos.y));
 
        if ((ua > 0) && (ua < 1) && (ub > 0) && (ub < 1))
        {
                anIntersectionPoint.x = aFirstLine.myStartPos.x + ua * (aFirstLine.myEndPos.x - aFirstLine.myStartPos.x);
                anIntersectionPoint.y = aFirstLine.myStartPos.y + ua * (aFirstLine.myEndPos.y - aFirstLine.myStartPos.y);
 
                return true;
        }
 
        return false;
	}


    bool LineVsSphere(const LineSegment3D& aLine,const Sphere& aSphere,Vector3f& anIntersectionPoint)
    {
    	Vector3f m = aLine.myStartPos - aSphere.myCenterPosition;
    	Vector3f len = aLine.myEndPos-aLine.myStartPos;
		float length = len.Length();
    	Vector3f dir = len/length;
    	float c = m.Dot(m) - aSphere.myRadiusSquared;
    	float b = m.Dot(dir);
    	if((c>0.0f) && (b>0.0f))
    	{
    		return false;
    	}
    	float discr = b*b - c;
    	if(discr<0.0f)
    	{
    		return false;
    	}
    	float t = -b - sqrt(discr);
    	if(t<0.0f)
    	{
    		t = 0.0f;
    	}
    	if(t<=length)
    	{
    		anIntersectionPoint = aLine.myStartPos + dir * t;
       		return true;
    	}
    	return false;
    }

    bool LineVsAABB(const LineSegment3D& aLine,const AABB& anAABB,Vector3f& anIntersectionPoint)
    {
    	Vector3f len = aLine.myEndPos-aLine.myStartPos;
    	float tmin = 0.0f;
    	float tmax = len.Length();
    	Vector3f dir = len/tmax;
    	if(dir.x==0)
    	{
    		if((aLine.myStartPos.x<anAABB.myMinPos.x) || (aLine.myStartPos.x>anAABB.myMaxPos.x))
    		{
    			return false;
    		}
    	}
    	else
    	{
    		float ood = 1.0f / dir.x;
    		float t1 = (anAABB.myMinPos.x-aLine.myStartPos.x) * ood;
    		float t2 = (anAABB.myMaxPos.x-aLine.myStartPos.x) * ood;
    		if(t1>t2)
    		{
    			float temp = t1;
    			t1 = t2;
    			t2 = temp;
    		}
    		if(t1>tmin)
    		{
    			tmin = t1;
    		}
    		if(t2<tmax)
    		{
    			tmax = t2;
    		}
    		if(tmin>tmax)
    		{
    			return false;
    		}
    	}
    	if(dir.y==0)
    	{
    		if((aLine.myStartPos.y<anAABB.myMinPos.y) || (aLine.myStartPos.y>anAABB.myMaxPos.y))
    		{
    			return false;
    		}
    	}
    	else
    	{
    		float ood = 1.0f / dir.y;
    		float t1 = (anAABB.myMinPos.y-aLine.myStartPos.y) * ood;
    		float t2 = (anAABB.myMaxPos.y-aLine.myStartPos.y) * ood;
    		if(t1>t2)
    		{
    			float temp = t1;
    			t1 = t2;
    			t2 = temp;
    		}
    		if(t1>tmin)
    		{
    			tmin = t1;
    		}
    		if(t2<tmax)
    		{
    			tmax = t2;
    		}
    		if(tmin>tmax)
    		{
    			return false;
    		}
    	}
    	if(dir.z==0)
    	{
    		if((aLine.myStartPos.z<anAABB.myMinPos.z) || (aLine.myStartPos.z>anAABB.myMaxPos.z))
    		{
    			return false;
    		}
    	}
    	else
    	{
    		float ood = 1.0f / dir.z;
    		float t1 = (anAABB.myMinPos.z-aLine.myStartPos.z) * ood;
    		float t2 = (anAABB.myMaxPos.z-aLine.myStartPos.z) * ood;
    		if(t1>t2)
    		{
    			float temp = t1;
    			t1 = t2;
    			t2 = temp;
    		}
    		if(t1>tmin)
    		{
    			tmin = t1;
    		}
    		if(t2<tmax)
    		{
    			tmax = t2;
    		}
    		if(tmin>tmax)
    		{
    			return false;
    		}
    	}
    	anIntersectionPoint = aLine.myStartPos + dir * tmin;
    	return true;
    }

    bool SphereVsSphere(const Sphere& aFirstSphere,const Sphere& aSecondSphere)
    {
    	float dist = (aFirstSphere.myCenterPosition-aSecondSphere.myCenterPosition).Length2();
		float radius = aFirstSphere.myRadius+aSecondSphere.myRadius;
    	return (dist<=(radius*radius));
    }

    bool SphereVsAABB(const Sphere& aSphere, const AABB& anAABB)
    {
		if (PointInsideAABB(anAABB, aSphere.myCenterPosition))
		{
			return true;
		}
    	Vector3f closest = ClosestPointAABB(aSphere.myCenterPosition, anAABB);
    	float dist = (aSphere.myCenterPosition-closest).Length2();
    	return (dist<aSphere.myRadiusSquared);
		//// X
		//Vector3f normal(1.0f, 0.0f, 0.0f);
		//Vector3f point = anAABB.myCenterPos + normal * anAABB.myExtents.x;
		//float distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}
		//normal.Set(-1.0f, 0.0f, 0.0f);
		//point = anAABB.myCenterPos + normal * anAABB.myExtents.x;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}
		//// Y
		//normal.Set(0.0f, 1.0f, 0.0f);
		//point = anAABB.myCenterPos + normal * anAABB.myExtents.y;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}
		//normal.Set(0.0f, -1.0f, 0.0f);
		//point = anAABB.myCenterPos + normal * anAABB.myExtents.y;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}
		//// Z
		//normal.Set(0.0f, 0.0f, 1.0f);
		//point = anAABB.myCenterPos + normal * anAABB.myExtents.z;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}
		//normal.Set(0.0f, 0.0f, -1.0f);
		//point = anAABB.myCenterPos + normal * anAABB.myExtents.z;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
		//if (distance<-aSphere.myRadius)
		//{
		//	return false;
		//}

		//return true;
    }

    bool AABBVsAABB(const AABB& aFirstAABB, const AABB& aSecondAABB)
    {
    	// X vs X
    	if(aFirstAABB.myMaxPos.x < aSecondAABB.myMinPos.x)
			return false;
		if(aFirstAABB.myMinPos.x > aSecondAABB.myMaxPos.x)
			return false;
    	// Y vs Y
    	if(aFirstAABB.myMaxPos.y < aSecondAABB.myMinPos.y)
			return false;
		if(aFirstAABB.myMinPos.y > aSecondAABB.myMaxPos.y)
			return false;
    	// Z vs Z
    	if(aFirstAABB.myMaxPos.z < aSecondAABB.myMinPos.z)
			return false;
		if(aFirstAABB.myMinPos.z > aSecondAABB.myMaxPos.z)
			return false;

		return true;
    }


    // Lab 2

    bool LineVsOBB(LineSegment3D& aLine,OBB& anOBB,Vector3f& anIntersectionPoint)
    {
		return false;
    }


    bool OBBVsPlane(OBB& anOBB,Plane& aPlane)
    {
    	bool sameSide1 = true, sameSide2 = true;
    	Planef p(aPlane.myPoint, aPlane.myNormal);
    	for(unsigned int index=0; index<8; ++index)
    	{
    		bool test = p.Inside(anOBB.myPoints[index]);
    		sameSide1 = sameSide1 && test;
    		sameSide2 = sameSide2 && !test;
    	}
    	return(sameSide1==sameSide2);
    }

    bool AABBVsPlane(AABB& anAABB,Plane& aPlane)
    {
    	float dist = (aPlane.myPoint-anAABB.myCenterPos).Length();
    	Vector3f absNormal(fabs(aPlane.myNormal.x), fabs(aPlane.myNormal.y), fabs(aPlane.myNormal.z));
    	float center = anAABB.myCenterPos.Dot(aPlane.myNormal);
    	float extent = anAABB.myExtents.Dot(absNormal);
    	return ((dist<(center-extent)) || (dist>(center+extent)));
    }

    bool SphereVsPlane(Sphere& aSphere,Plane& aPlane)
    {
    	float dist = aPlane.myNormal.Dot(aSphere.myCenterPosition);
    	if(dist>aSphere.myRadius)
			return false;
		return true;
    }

	bool LineVsPlane(const LineSegment3D& aLine,const Plane& aPlane, Vector3f& anIntersectionPoint)
	{
		Vector3f u = aLine.myEndPos-aLine.myStartPos;
		if (aPlane.myNormal.Dot(u)==0)
		{
			return false;
		}
		float s = -aPlane.myNormal.Dot(aLine.myStartPos-aPlane.myPoint)/aPlane.myNormal.Dot(u);
		if ((s<0) || (s>1))
		{
			return false;
		}
		anIntersectionPoint = aLine.myStartPos + u * s;
		return true;
	}


    bool LineVsTriangle(const LineSegment3D& aLine, const Triangle& aTriangle,Vector3f& anIntersectionPoint)
    {
    	Vector3f e1 = aTriangle.myPoints[1] -  aTriangle.myPoints[0];
    	Vector3f e2 = aTriangle.myPoints[2] -  aTriangle.myPoints[0];
    	Vector3f s = aLine.myStartPos - aTriangle.myPoints[0];
    	Vector3f d = aLine.myStartPos - aLine.myEndPos;

		Vector3f n = e2.Cross(e1);

    	float det = d.Dot(n);
    	if(det <= 0)
			return false;

		float t = s.Dot(n);
		if ((t < 0) || (t > det))
		{
			return false;
		}

		Vector3f e = s.Cross(d);
		float v = e2.Dot(e);
		if ((v<0.0f) || (v>det))
		{
			return false;
		}
		float w = -e1.Dot(e);
		if ((w<0) || (v+w>det))
		{
			return false;
		}

		Plane p;
		p.myPoint = aTriangle.myPoints[0];
		p.myNormal = n;

		return LineVsPlane(aLine, p, anIntersectionPoint);
    }


    bool OBBVsAABB(OBB& anOBB,AABB& anAABB)
    {
		return false;
    }

    bool OBBVsOBB(OBB& aFirstOBB,OBB& aSecondOBB)
    {
		return false;
    }
};
#pragma warning (pop)